/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * queue.cxx
 *
 * PURPOSE:
 *   Implemnts a fifo queue for floats
 *
 * CREATED:
 * 11/2000 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Author: dixonc $
 * $Date: 2005/07/13 21:00:06 $
 * $Revision: 1.3 $
 *
 ***********************************************************************/

#include "queue.h"
#include <assert.h>
#include <string.h>
#include <stdio.h>
#include <float.h>

// be very careful using this, don't abuse it
#ifndef min
#	define min(size1, size2) ( size1 < size2 ? size1 : size2)
#endif

Queue::Queue(size_t numElements)
{
	NumElements = numElements;

	PushInd = 0; PopInd = 0;
	NumUsed = 0;

	// get memory
	Memory = new QueueType[NumElements];
	assert(Memory);

	minVal.val = FLT_MAX;
	maxVal.val = FLT_MIN;

	for(unsigned int i=0; i < numElements; i++ )
		Memory[i] = 0;

	// initialize semaphore
#ifndef VXWORKS
	if( sem_init(&Mutex, 0, 1) != 0)
	{
		perror("Queue::Queue() - sem_init");
		assert(false);
	}
#endif
}

Queue::~Queue()
{
#ifndef VXWORKS
	(void)sem_destroy(&Mutex);
#endif

	delete [] Memory;
}

bool Queue::Push(const QueueType * element, size_t numElements)
{
	size_t oldInd;
	int free=0;

	if(element == NULL )
		return false;

#ifndef VXWORKS
	sem_wait( &Mutex );
#endif

	for( size_t i=0; i < numElements; i++)
	{
		// num free
		free = NumElements - NumUsed;

		// pop of an element
		if( free == 0 ) {
			oldInd = PopInd;

			PopInd = IncInd(PopInd);

			if( oldInd == minVal.ind )
				findMin();
			else if( oldInd == maxVal.ind )
				findMax();
		} else
			NumUsed++;

		Memory[PushInd] = element[i];

		PushInd = IncInd(PushInd);
	}

#ifndef VXWORKS
	sem_post( &Mutex );
#endif

	// for now, remove later
	findMax();
	// for now, remove later
	findMin();

	return true;
}

void Queue::findMin()
{
	minVal.val = FLT_MAX;
	for( size_t i=0; i < NumUsed; i++)
	{
		size_t ind  = (i + PopInd ) % NumElements;
		if( minVal.val > Memory[ind]){
			minVal.val = Memory[ind];
			minVal.ind = ind;
		}
	}
}

void Queue::findMax()
{
	maxVal.val = FLT_MIN;
	for( size_t i=0; i < NumUsed; i++)
	{
		size_t ind  = (i + PopInd ) % NumElements;
		if( maxVal.val < Memory[ind] ){
			maxVal.val = Memory[ind];
			maxVal.ind = ind;
		}
	}
}

QueueType Queue::Max() const
{
	return maxVal.val;
}
QueueType Queue::Min() const
{
	return minVal.val;
}
QueueType Queue::Average() const
{
	QueueType sum = 0;
	for( size_t i=0; i < NumUsed; i++) {
		size_t ind  = (i + PopInd ) % NumElements;
		sum += Memory[ind];
	}
	return sum / NumUsed;
}

size_t Queue::Pop(QueueType * element, size_t numElements)
{

	size_t maxPop = min(numElements, NumUsed );

	if( maxPop != 0)
	{
#ifndef VXWORKS
		sem_wait( &Mutex);
#endif
		NumUsed -= maxPop;

#ifndef VXWORKS
		sem_post( &Mutex);
#endif
		size_t oldInd;
		for( size_t i=0; i < maxPop; i++ )
		{
			oldInd = PopInd;

			if( element != NULL)
				element[i] = Memory[PopInd];

			PopInd = IncInd(PopInd);

			if( oldInd == minVal.ind )
				findMin();
			else if( oldInd == maxVal.ind )
				findMax();
		}
	}

	return maxPop;
}


QueueType* const Queue::Peek(size_t elementInd ) 
{
	if( elementInd > Used() )
		return NULL;
	else
	{
		int ind  = (elementInd + PopInd ) % NumElements;
		return &(Memory[ind]);
	}
}

size_t Queue::Size() const
{
	return NumElements;
}

void Queue::SetSize(size_t numElements )
{
#ifndef VXWORKS
	sem_wait( &Mutex);
#endif
	if( Memory )
		delete [] Memory;

	PushInd = 0; PopInd = 0;
	NumUsed = 0;

	NumElements = numElements;
	Memory = new QueueType[NumElements];
	assert(Memory);

#ifndef VXWORKS
	sem_post( &Mutex);
#endif
}

size_t Queue::Free() 
{
#ifndef VXWORKS
	sem_wait( &Mutex);
#endif

	int val = NumElements - NumUsed;

#ifndef VXWORKS
	sem_post( &Mutex);
#endif

	return val;
}
size_t Queue::Used() 
{

#ifndef VXWORKS
	sem_wait( &Mutex);
#endif

	int val = NumUsed;

#ifndef VXWORKS
	sem_post( &Mutex);
#endif

	return val;
}

size_t Queue::IncInd(size_t ind, size_t numInc)
{
	return( (ind + numInc) % NumElements );
}

// if saving to a file that already exsists, then
// simply append the new data to it.  This 
// allows for dumping of save data in queue to
// file for recovery.
bool Queue::saveToFile(const char * fileName)
{
	if( fileName == NULL || strcmp(fileName, "") == 0)
		return false;

	ofstream outputFile(fileName, ios::out | ios::app);
	if( !outputFile )  {
		perror("Queue::saveToFile() could not open file");
		return false;
	} else {
		for(size_t i=0; i < NumUsed; i++)
			outputFile << *(this->Peek(i)) << endl;

		outputFile.close();
		return true;
	}
}
